#include "mini_car2.h"
#include "wheel.h"
#include "math.h"




void Mini_Car_Run(FOOD_CAR *car)
{
	motor_wheel_run(car);

}
 void Mini_Car_Stop(FOOD_CAR *car)
 {
	 motor_wheel_stop(car);
 }
 
 void Mini_Car_Forward_Setup(FOOD_CAR *car)
 {
		motor_wheel_forward_setup(car);
	
 }
 
 void Mini_Car_Backward_Setup(FOOD_CAR *car)
 {
		motor_wheel_backward_setup(car);
	
 }
 
 
 void Mini_Car_RightShift_Setup(FOOD_CAR *car)
 {
		motor_wheel_RightShift_setup(car);
	
 }
 
 void Mini_Car_LeftShift_Setup(FOOD_CAR *car)
 {
		motor_wheel_LeftShift_setup(car);
	
 }
 void Mini_Car_CW_Turn(FOOD_CAR *car)
 {
	 motor_wheel_CW_turn(car);
 }
 
 void Mini_Car_CCW_Turn(FOOD_CAR *car)
 {
	 motor_wheel_CCW_turn(car);
 
 }
 

 
 void Food_Car_Init(FOOD_CAR *car,float kp,int8_t max_speed,float spin_kp,int8_t max_angleSpeed)
 {
	 car->angle_speed=0;
	 car->run_speed=0;
	 car->max_speed=max_speed;
	 car->max_angle_speed=max_angleSpeed;
	 car->min_angle_speed=max_angleSpeed*(-1);
	 car->speed_pid_kp=kp;
	 car->CW_Turn_kp=spin_kp;
	 car->wheel_4_speed=0;
	 car->wheel_1_speed=0;
	 car->wheel_2_speed=0;
	 car->wheel_3_speed=0;
	 car->taget_coordinate_z=0.0f;
	 car->target_coordinate_x=0.0f;
	 car->target_coord_atanNum=0.0f;

 }
  void Food_Car_EmergencyStop(FOOD_CAR *food_car)
 {
	 		  food_car->wheel_1_speed=food_car->wheel_2_speed=food_car->wheel_3_speed=food_car->wheel_4_speed=food_car->run_speed=0;
	      food_car->angle_speed=0;
				HAL_GPIO_WritePin(Emegency_Stop_GPIO_Port,Emegency_Stop_Pin,GPIO_PIN_RESET);

 }
 
 
  void Food_Car_BluetoothControl(FOOD_CAR *food_car,T_Command cmd)
 {
		food_car->run_speed=cmd.speed_amount;
		if(food_car->run_speed>food_car->max_speed)
			food_car->run_speed=food_car->max_speed;
		else if(food_car->run_speed<0)
			food_car->run_speed=0;
		
		switch((int)cmd.action)
		{
			case CMD_BLUETOOTH_FORWARD:
			//	UART5_Printf("%d\r\n",CMD_BLUETOOTH_FORWARD);
				Mini_Car_Forward_Setup(food_car);
			break;
			case CMD_BLUETOOTH_BACKWARD:
				//UART5_Printf("%d\r\n",CMD_BLUETOOTH_BACKWARD);
				Mini_Car_Backward_Setup(food_car);
			break;
			case  CMD_BLUETOOTH_CCW:
				//UART5_Printf("%d\r\n",CMD_BLUETOOTH_CCW);
				Mini_Car_CCW_Turn(food_car);
			break;
			case CMD_BLUETOOTH_CW:
			//	UART5_Printf("%d\r\n",CMD_BLUETOOTH_CW);
				Mini_Car_CW_Turn(food_car);
			break;
			case CMD_BLUETOOTH_SHIFTLEFT:
				//UART5_Printf("%d\r\n",CMD_BLUETOOTH_SHIFTLEFT);
				Mini_Car_LeftShift_Setup(food_car);
			break;
			case CMD_BLUETOOTH_SHIFTRIGHT:
				//UART5_Printf("%d\r\n",CMD_BLUETOOTH_SHIFTRIGHT);
				Mini_Car_RightShift_Setup(food_car);
				break;
			default:
				break;
		}

 }
 uint8_t Get_CarState_Lidar(Lidar_Type* lidar1,int8_t run_speed,int right_length,int left_length)
 {
	  uint16_t y_boundary=0;
	 	uint8_t stop_state=0;
				
		y_boundary=run_speed*BOUNDARY_RATIO+BASEBOUNDARY;
	 for(uint16_t angle=0;angle<361;angle++)
	{
		uint8_t zone=angle/90;
		uint8_t obstacle_para_forward=0;
//				uint8_t obstacle_para_turn=0;
		switch(zone)
		{

			case 0:
				obstacle_para_forward=Detect_Obstacles(lidar1,LIDAR_LOCATION_ORIGINAL,right_length,y_boundary,OBSTACLE_MAX_DIS,angle);
				if(obstacle_para_forward==1)
				{
					stop_state=1;
					//UART5_Printf("There is a obstacle\r\n");
				
				}
				break;

			case 3:
				obstacle_para_forward=Detect_Obstacles(lidar1,left_length,LIDAR_LOCATION_ORIGINAL,y_boundary,OBSTACLE_MAX_DIS,angle);
			 if(obstacle_para_forward==1)
					stop_state=1;				
	
				break;
			default: 
				break;
			
		}
		if(stop_state==1)
			break;

	}
	return stop_state;

 }
 
 void Car_Joystick_Mode_Init(FOOD_CAR *car,float kp,int8_t max_speed)
 {
	 car->angle_speed=0;
	 car->run_speed=0;
	 car->max_speed=max_speed;
	 car->speed_pid_kp=kp;

	 car->wheel_4_speed=0;
	 car->wheel_1_speed=0;
	 car->wheel_2_speed=0;
	 car->wheel_3_speed=0;
	 car->taget_coordinate_z=0.0f;
	 car->target_coordinate_x=0.0f;
	 car->target_coord_atanNum=0.0f;

 }
 
 void Car_Joystick_Control_Mode(FOOD_CAR *car,Joystick_Command cmd)
 {
	 double target_runSpeed,target_spinSpeed;
	 int radius,angle;
	 double absolute_run_speed,absolute_spin_speed;
	 
	 int8_t Direction_flag,run_speed_flag,spin_speed_flag;
	 double radian;
	 	 if(cmd.mode==SPIN_MODE)
		 {
		 	 radius=cmd.Radius_Right;
		   angle=cmd.Angle_Right;
		 }

		 else if(cmd.mode==SHIFT_MODE)
		 {
		   radius=cmd.Radius_Left;
		   angle=cmd.Angle_Left;
		 }
		 
	 angle=360-angle*10;
	 if(angle>=90&&angle<110)
		 angle=90;
	 else if(angle>=250&&angle<270)
		 angle=270;
	 if((angle>=0&&angle<=90)||(angle>270&&angle<=360))
		 Direction_flag=1;
	 else
		 Direction_flag=-1;
	 
//	 uint8_t zone=angle/90;
//	 switch(zone)
//	 {
//		 case 0:
//		 case 3:
//		 case 4:
//			 run_speed_flag=1;
//			 break;
//		 case 1:
//		 case 2:
//			 run_speed_flag=-1;
//			 break;		 
//		 default:
//			 run_speed_flag=0;
//			 break;
//	 }
		 

	 radian =angle*PI/180;
	 target_spinSpeed=radius*sin(radian);
	 target_runSpeed=radius*cos(radian);
	 
	 if(target_runSpeed>=0)
		 run_speed_flag=1;
	 else
		 run_speed_flag=-1;
	 if(target_spinSpeed>=0)
		 spin_speed_flag=1;
	 else
		 spin_speed_flag=-1;
    absolute_run_speed=fabs(target_runSpeed);
	  absolute_spin_speed=fabs(target_spinSpeed);
	 
		car->run_speed=car->run_speed*car->speed_pid_kp+(1-car->speed_pid_kp)*SpeedOutput(absolute_run_speed, Min_Radius_Input ,Min_Speed, Max_Radius_Input,car->max_speed)*run_speed_flag;
    car->angle_speed=car->angle_speed*car->CW_Turn_kp+(1-car->CW_Turn_kp)*SpeedOutput(absolute_spin_speed,Min_Radius_Input , Min_Speed,Max_Radius_Input, car->max_speed)*spin_speed_flag;
	  

	 if(cmd.mode==SPIN_MODE)
	 {		 
		 if(Direction_flag==1)
		 {
			 car->wheel_2_speed=car->run_speed+car->angle_speed;
       car->wheel_1_speed=car->run_speed-car->angle_speed;
		 
		 }
		 else
		 {
			 	car->wheel_1_speed=car->run_speed+car->angle_speed;
        car->wheel_2_speed=car->run_speed-car->angle_speed;
		 
		 }


    car->wheel_3_speed=car->wheel_1_speed;
    car->wheel_4_speed=car->wheel_2_speed;
	 
	 }
	 else if(cmd.mode==SHIFT_MODE)
	 {
		 if(Direction_flag==1)
		 {
			 car->wheel_1_speed=car->run_speed+car->angle_speed;
       car->wheel_2_speed=car->run_speed-car->angle_speed;
		 
		 }
		 else
		 {
			 	car->wheel_2_speed=car->run_speed+car->angle_speed;
        car->wheel_1_speed=car->run_speed-car->angle_speed;
		 
		 }

    car->wheel_4_speed=car->wheel_1_speed;
    car->wheel_3_speed=car->wheel_2_speed;
	 
	 }
	 
	// my_printf("radius:%d,angle:%d,wheel speed:%d,%d,%d,%d\r\n",radius,angle,car->wheel_1_speed,car->wheel_2_speed,car->wheel_3_speed,car->wheel_4_speed);
	 
 
 }
 
 
 float SpeedOutput(float input, float minInput, float minSpeed, float maxInput, float maxSpeed)
{
    if(input<=minInput)
        return minSpeed;
    if(input>=maxInput)
        return maxSpeed;
    return minSpeed+(input-minInput)*(maxSpeed-minSpeed)/(maxInput-minInput);
}
 

void xyz2speed2(FOOD_CAR *car)
{
	
	  car->run_speed=car->run_speed*car->speed_pid_kp+(1-car->speed_pid_kp)*SpeedOutput(car->taget_coordinate_z, Min_Distance,Min_Speed, Max_Distance,car->max_speed);
    car->angle_speed=car->angle_speed*car->CW_Turn_kp+(1-car->CW_Turn_kp)*SpeedOutput(car->target_coord_atanNum,Min_angle , car->min_angle_speed,Max_angle, car->max_angle_speed);
	
    car->wheel_2_speed=car->run_speed+car->angle_speed;
    car->wheel_1_speed=car->run_speed-car->angle_speed;

    car->wheel_3_speed=car->wheel_1_speed;
    car->wheel_4_speed=car->wheel_2_speed;
} 
 


